# 力矩到电流的映射转换
# import matplotlib.pyplot as plt
import pandas as pd
import numpy as np
from os.path import join, dirname

# 读取CSV文件
data = pd.read_csv(join(dirname(__file__), "wh_cali.csv"), header=None)

# 分离数据
x = data[0]
y = data[1]

# 选择正电流数据
x_positive = x[x > 0]
y_positive = y[x > 0]

# 选择负电流数据
x_negative = x[x < 0]
y_negative = y[x < 0]

coef_positive = np.polyfit(x_positive, y_positive, 1)
coef_negative = np.polyfit(x_negative, y_negative, 1)

slope_positive, intercept_positive = coef_positive
slope_negative, intercept_negative = coef_negative

c_positive_intercept = abs(-intercept_positive / slope_positive)
c_negative_intercept = -abs(-intercept_negative / slope_negative)


def trans_t2c(t):
    t_bound = 0.3
    if t >= t_bound:
        c = c_positive_intercept + t/slope_positive
    elif t <= -t_bound:
        c = c_negative_intercept + t/slope_negative
    else:
        c_p = c_positive_intercept + t_bound/slope_positive
        c_n = c_negative_intercept - t_bound/slope_negative
        slope_zero = (c_p - c_n)/(2*t_bound)
        intercept_zero = (c_p + c_n)/2
        c = slope_zero*t + intercept_zero
    bound = 1000
    c = round(np.clip(c, -bound, bound))
    return c

# print(trans_t2c(-2.15))
# t_plot = np.linspace(-6, 6, 1000)
# c_plot = [trans_t2c(t) for t in t_plot]
# plt.plot(c_plot, t_plot)


# # 添加标题和标签
# plt.title('whj calibration')
# plt.xlabel('current(mA)')
# plt.ylabel('torque(Nm)')

# # 显示网格
# plt.grid(True)

# # 保存图形
# plt.savefig("wh_cali.png")

# # 显示图形
# plt.show()